package org.lovelandhs.robotics.y2010.camera;

import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.camera.AxisCameraException;
import edu.wpi.first.wpilibj.image.ColorImage;
import edu.wpi.first.wpilibj.image.NIVisionException;

/**
 *
 * @author Robotics
 */
public class Camera {

	public static final double THRESHOLD = 0.01;

	private AxisCamera camera;
	private TrackerDashboard dashboard;
	private Gyro gyro;
	private Servo pan;
	private Servo tilt;

	public Camera() {
		init();
	}

	private void init() {
		camera = AxisCamera.getInstance();
		camera.writeResolution(AxisCamera.ResolutionT.k320x240);
		camera.writeBrightness(0);

		dashboard = new TrackerDashboard();
		gyro = new Gyro(1);
		pan = new Servo(3);
		tilt = new Servo(4);
	}

	public void turn() {
		try {
			if (camera.freshImage()) {// && turnController.onTarget()) {
				double gyroAngle = gyro.pidGet();
				ColorImage image = camera.getImage();
				Thread.yield();
				Target[] targets = Target.findCircularTargets(image);
				Thread.yield();
				image.free();
				if (targets.length == 0 || targets[0].m_score < THRESHOLD) {
					System.out.println("No target found");
					Target[] newTargets = new Target[targets.length + 1];
					newTargets[0] = new Target();
					newTargets[0].m_majorRadius = 0;
					newTargets[0].m_minorRadius = 0;
					newTargets[0].m_score = 0;
					for (int i = 0; i < targets.length; i++) {
						newTargets[i + 1] = targets[i];
					}
					dashboard.updateVisionDashboard(0.0, gyro.getAngle(), 0.0, 0.0, newTargets);
				} else {
					System.out.println(targets[0]);
					System.out.println("Target Angle: " + targets[0].getHorizontalAngle());
					//turnController.setSetpoint(gyroAngle + targets[0].getHorizontalAngle());
					dashboard.updateVisionDashboard(0.0, gyro.getAngle(), 0.0, targets[0].m_xPos / targets[0].m_xMax, targets);

					pan.setAngle(targets[0].getHorizontalAngle());
					//tilt.setAngle(targets[0].get);
				}
			}
		} catch (NIVisionException ex) {
			ex.printStackTrace();
		} catch (AxisCameraException ex) {
			ex.printStackTrace();
		}
	}
}
